#include "speed_cntr.h"
#include "user_device.h"
#include "modbus.h"
#include "stdlib.h"


//! Cointains data for timer interrupt.
SpeedRampData srd[MACHINE_COUNT] = {0};

SRD_DESC srd_desc[MACHINE_COUNT] ={
    { TIMER3,  TIMER_CH_0, IO_S1_HOME,  IO_IMPORTL_READY,   IO_S1_LIMIT,    NULL_IN,    &srd[S1] },
    { TIMER11, TIMER_CH_1, IO_S2_HOME,  IO_IMPORTR_READY,   IO_S2_LIMIT,    NULL_IN,    &srd[S2] },
    { TIMER9,  TIMER_CH_0, IO_V1_HOME,  NULL_IN,            NULL_IN,        NULL_IN,    &srd[V1] },
    { TIMER7,  TIMER_CH_3, IO_V2_HOME,  NULL_IN,            NULL_IN,        NULL_IN,    &srd[V2] },
    { TIMER8,  TIMER_CH_0, IO_V3_HOME,  IO_V3_LIMIT,        NULL_IN,        NULL_IN,    &srd[V3] },
    { TIMER0,  TIMER_CH_0, NULL_IN,     IO_EXPORT_READY,    NULL_IN,        NULL_IN,    &srd[T1] },
    { TIMER2,  TIMER_CH_2, NULL_IN,     NULL_IN,            NULL_IN,        NULL_IN,    &srd[R1] },
    { TIMER13, TIMER_CH_0, IO_R2_HOME,  NULL_IN,            NULL_IN,        NULL_IN,    &srd[R2] },
};

// 设置电机细分,并使能
void Set_Micro(void)
{
}

void InitMachine(void)
{
    int i;
    srd[S1].role = S1;
    srd[S2].role = S2;
    srd[V1].role = V1;
    srd[V2].role = V2;
    srd[V3].role = V3;
    srd[T1].role = T1;
    srd[R1].role = R1;
    srd[R2].role = R2;
    
    srd[S1].reset_step = S_RESET_BUFFER_STEP;
    srd[S2].reset_step = S_RESET_BUFFER_STEP;
    srd[V1].reset_step = V_RESET_BUFFER_STEP;
    srd[V2].reset_step = V_RESET_BUFFER_STEP;
    srd[V3].reset_step = V3_RESET_BUFFER_STEP;
    srd[R2].reset_step = R2_RESET_BUFFER_STEP;
    
    srd[S1].limit_step = S_LIMIT_BUFFER_STEP;
    srd[S2].limit_step = S_LIMIT_BUFFER_STEP;
    srd[V1].limit_step = V_LIMIT_BUFFER_STEP;
    srd[V2].limit_step = V_LIMIT_BUFFER_STEP;
    srd[V3].limit_step = V3_LIMIT_BUFFER_STEP;
    srd[T1].limit_step = T1_IMPORT_LIMIT_BUFFER_STEP;

    srd[S1].limit2_step = S_LIMIT2_BUFFER_STEP;
    srd[S2].limit2_step = S_LIMIT2_BUFFER_STEP;
    srd[V1].limit2_step = srd[V1].limit_step;
    srd[V2].limit2_step = srd[V2].limit_step;
    srd[V3].limit2_step = srd[V3].limit_step;
    srd[T1].limit2_step = srd[T1].limit_step;

    srd[S1].limit3_step = srd[S1].limit_step;
    srd[S2].limit3_step = srd[S2].limit_step;
    srd[V1].limit3_step = srd[V1].limit_step;
    srd[V2].limit3_step = srd[V2].limit_step;
    srd[V3].limit3_step = srd[V3].limit_step;
    srd[T1].limit3_step = srd[T1].limit_step;

    srd[S1].limit_target = rw_word.mp[S1].max_distance;
    srd[S2].limit_target = rw_word.mp[S2].max_distance;
    srd[V1].limit_target = rw_word.v1_close_pos;
    srd[V2].limit_target = rw_word.v2_close_pos;
    srd[V3].limit_target = rw_word.v3_close_pos;
    srd[T1].limit_target = rw_word.mp[T1].max_distance;

    for (i = TRACK_EXIT_MOTOR_SITE_FIRST; i< TRACK_EXIT_MOTOR_SITE_ORIGIN; i++)
        TrackExitMotor_coordinate[i] = rw_word.scan_pos[i - TRACK_EXIT_MOTOR_SITE_FIRST];
    TrackExitMotor_coordinate[TRACK_EXIT_MOTOR_SITE_ORIGIN] = 1900;
    srd[S1].micro = 0;
    srd[S2].micro = 0;
    srd[V1].micro = V_MISCRO;
    srd[V2].micro = V_MISCRO;
    srd[V3].micro = V_MISCRO;
    srd[T1].micro = 0;

    srd[S1].coder_self = 0;
    srd[S2].coder_self = 0;
    srd[V1].coder_self = 1;
    srd[V2].coder_self = 1;
    srd[V3].coder_self = 1;
    srd[T1].coder_self = 0;

    Set_Micro();

    IO_Set(IO_S1_LOCK);
    IO_Set(IO_S2_LOCK);
    IO_Set(IO_V1_LOCK);
    IO_Set(IO_V2_LOCK);
    IO_Set(IO_V3_LOCK);
    IO_Set(IO_T1_LOCK);
    IO_Set(IO_R1_LOCK);
    IO_Set(IO_R2_LOCK);

    IO_Reset(IO_S1_EN);
    IO_Reset(IO_S2_EN);
    IO_Reset(IO_V1_EN);
    IO_Reset(IO_V2_EN);
    IO_Reset(IO_V3_EN);
    IO_Reset(IO_T1_EN);
    IO_Reset(IO_R1_EN);
    IO_Reset(IO_R2_EN);
    IO_Reset(IO_STRIP_LED);
}

void speed_cntr_Move(SpeedRampData *srd, long step, unsigned int speed)
{
    if (step == 0)
        return;

    // Set direction from sign on step value.
    if(step < 0)
    {
        srd->dir = CCW;
        srd->step_offset = -1;
        step = -step;
    }
    else
    {
        srd->dir = CW;
        srd->step_offset = 1;
    }

    // 最大速度对应的速度表索引
    // n = speed^2 / (2*alpha*accel)
    srd->max_index = SPEED2INDEX(speed);
    if (srd->max_index > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        srd->max_index = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;
    if (srd->first_stage_speed  > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        srd->first_stage_speed  = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;
    if (srd->second_stage_speed > (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1)
        srd->second_stage_speed = (SPEED_TABLE_SIZE << SPEED_INTERVAL_CODE) - 1;

    srd->speed_index = 0;
    srd->step_left = step;
    srd->run_state = MOTION_STATE_RUN;

    srd->running = 1;
    srd->need_cleanup = 0;
    srd->start_time = SystemTick();
    srd->step_count = 0;
    srd->init(srd->role);
}

void speed_update(uchar role)
{
    SpeedRampData *srd = srd_desc[role].srd;
    
    if(srd->coder_self)
    {
        if( ++srd->micro_step < (srd->micro >> 1))
            return;
        srd->micro_step = 0;
    }

    Set_Timer(role);

    if (srd->run_state == MOTION_STATE_STOP)
    {
        Stop_Timer(role);
        srd->need_cleanup = 1;
    }
    else
    {

        // 更新剩余步数
        srd->step_left--;
        srd->step_count++;

        // 走一步, 调用更新函数, 用以检查传感器状态, 更新剩余步数等
        srd->update(role);

        if (srd->step_left <= 0)
        {
            srd->run_state = MOTION_STATE_STOP;
        }
        else
        {
            // 更新下一步速度
            if (srd->speed_index > srd->step_left)
                srd->speed_index -= srd->speed_index / (ushort)srd->step_left;
            if (srd->speed_index > srd->max_index)
                srd->speed_index--;
            else if (srd->speed_index < srd->max_index)
                srd->speed_index++;
        }
    }
}


